(* # ===================================================================
   # Matrix Project
   # Copyright FEM-NUAA.CN 2020
   # =================================================================== *)


Require Export Reals.
Require Export List.
Import ListNotations.
Require Export Matrix.Mat.RMatrix.
Require Export Matrix.Mat.RMtacs.
Require Export Matrix.Mat.Mat_make.


Open Scope R_scope.

Definition Real := R.

Ltac f_equal2 :=
  f_equal; f_equal.

Ltac f_equal3 :=
  f_equal; f_equal; f_equal.

Section CoordinateTransformationMatrix.

Variable theta phi psi: R. (* 俯仰角和滚转角 *) 

(**************************    Rtheta : b to e   ********************************)

(* 在俯仰方向旋转theta角后机体坐标系到地球固连坐标系的转换矩阵(类型:矩阵 RMat 3 3) *)
Definition Rbe_theta:= mkMat_3_3  
    (cos theta)   0   (sin theta)
         0        1       0
  (- sin theta)   0   (cos theta).

(**************************    Rphi : b -> e   ********************************)

(* 在滚转方向旋转phi角后机体坐标系到地球固连坐标系的转换矩阵(类型:矩阵 RMat 3 3) *)
Definition Rbe_phi:= mkMat_3_3  
    1     0        0    
    0  (cos phi)  (-sin phi)
    0  (sin phi)  (cos phi).


(**************************    Rpsi : b -> e   ********************************)

(* 在偏航方向旋转psi角后机体坐标系到地球固连坐标系的转换矩阵(类型:矩阵 RMat 3 3) *)
Definition Rbe_psi := mkMat_3_3
   (cos psi)  (-sin psi)  0
   (sin psi)  (cos psi)   0 
       0         0        1 .

(**************************   Rtheta * Rphi : b -> e   ********************************)

Definition Rbe_theta_phi := Rbe_theta RM* Rbe_phi.

Definition Rbe_theta_phi_re := mkMat_3_3
  (cos theta)  (sin theta * sin phi) (sin theta * cos phi)
        0            (cos phi)             (- sin phi)
  (-sin theta) (cos theta * sin phi) (cos theta * cos phi).

Lemma Rbe_theta_phi_eq :  Rbe_theta_phi === Rbe_theta_phi_re.
Proof. RMat_mul_simpl. unfold mkMat_3_3'.
  reflexivity. Qed. 

(**************************   Rtheta * Rpsi : b -> e   ********************************)

Definition Rbe_theta_psi :=  Rbe_theta RM* Rbe_psi.

Definition Rbe_theta_psi_re :=mkMat_3_3
  (cos theta *cos psi ) (- cos theta * sin psi) (sin theta) 
       (sin psi )               (cos psi)            0 
  ( -sin theta * cos psi) (sin theta * sin psi) (cos theta).

Lemma Rbe_theta_psi_eq :  Rbe_theta_psi === Rbe_theta_psi_re.
Proof. RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal3. ring. f_equal2. ring. Qed.

(**************************   Rphi * Rtheta : b -> e   ********************************)

Definition Rbe_phi_theta := Rbe_phi RM* Rbe_theta.

Definition Rbe_phi_theta_re :=mkMat_3_3
        (cos theta)          0           ( sin theta )
  (sin phi * sin theta) (cos phi ) (-sin phi * cos theta)
  (-cos phi * sin theta)(sin phi )( cos phi * cos theta).

Lemma Rbe_phi_theta_eq : Rbe_phi_theta === Rbe_phi_theta_re.
Proof. RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal3. ring. f_equal. ring. Qed.

(**************************   Rphi * Rpsi : b -> e   ********************************)

Definition Rbe_phi_psi := Rbe_phi RM* Rbe_psi.

Definition Rbe_phi_psi_re := mkMat_3_3
  ( cos psi) ( - sin psi ) 0 
  (cos phi * sin psi )( cos phi * cos psi )( - sin phi )
  ( sin phi * sin psi)( sin phi * cos psi )(  cos phi ).

Lemma Rbe_phi_psi_eq : Rbe_phi_psi === Rbe_phi_psi_re.
Proof. RMat_mul_simpl.  unfold mkMat_3_3'.
  reflexivity. Qed.

(**************************   Rpsi * Rtheta : b -> e   ********************************)

Definition Rbe_psi_theta := Rbe_psi RM* Rbe_theta.

Definition Rbe_psi_theta_re := mkMat_3_3
  (cos psi * cos theta) (- sin psi )( cos psi * sin theta)
  (sin psi * cos theta) ( cos psi ) ( sin psi * sin theta)
  ( -sin theta ) 0 ( cos theta ).

Lemma Rbe_psi_theta_eq : Rbe_psi_theta === Rbe_psi_theta_re.
Proof. RMat_mul_simpl. unfold mkMat_3_3'.
  reflexivity. Qed.

(**************************   Rpsi * Rphi : b -> e   ********************************)

Definition Rbe_psi_phi :=  Rbe_psi RM* Rbe_phi.

Definition Rbe_psi_phi_re :=mkMat_3_3
  ( cos psi )  (-sin psi * cos phi )( sin psi * sin phi )
  (sin psi )  (cos psi * cos phi ) (- cos psi * sin phi )
   0 ( sin phi )( cos phi ).


Lemma Rbe_psi_phi_eq : Rbe_psi_phi === Rbe_psi_phi_re.
Proof. RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal3. f_equal. ring. f_equal2. ring. Qed.

(**************************   Rtheta * Rphi * Rpsi : b -> e   ********************************)

Definition Rbe_theta_phi_psi := (Rbe_theta RM* Rbe_phi) RM* Rbe_psi.

Definition Rbe_theta_phi_psi_re:= mkMat_3_3
  (cos theta * cos psi + sin theta * sin phi * sin psi )
  (  - cos theta * sin psi + sin theta * sin phi *cos psi) 
  (  sin theta * cos phi)
  ( cos phi * sin psi )( cos phi * cos psi )( -sin phi)
  ( -sin theta * cos psi + cos theta * sin phi *sin psi)
  (  sin theta * sin psi + cos theta * sin phi * cos psi)
  (   cos theta * cos phi).

Lemma Rbe_theta_phi_psi_eq : Rbe_theta_phi_psi === Rbe_theta_phi_psi_re .
Proof. RMat_mul_simpl. unfold mkMat_3_3'.
  f_equal3. ring. ring. f_equal. ring.
  f_equal2. ring. 
Qed.

(**************************   Rtheta * Rpsi * Rphi : b -> e   ********************************)

Definition Rbe_theta_psi_phi := (Rbe_theta RM* Rbe_psi) RM* Rbe_phi.

Definition Rbe_theta_psi_phi_re:= mkMat_3_3
  ( cos theta * cos psi )
  (   -cos theta *sin psi * cos phi + sin theta *sin phi)
  (   cos theta * sin psi * sin phi + sin theta * cos phi)
  ( sin psi)( cos psi * cos phi )( - cos psi *sin phi)
  ( - sin theta * cos psi) 
  (   sin theta *sin psi * cos phi + cos theta * sin phi )
  (   -sin theta*sin psi * sin phi + cos theta * cos phi ).

Lemma Rbe_theta_psi_phi_eq : Rbe_theta_psi_phi === Rbe_theta_psi_phi_re .
Proof. RMat_mul_simpl. unfold mkMat_3_3'. 
  f_equal3. ring. f_equal. ring. f_equal. ring.
  f_equal. ring. f_equal2. ring. f_equal. ring. 
Qed.


(**************************   Rphi * Rtheta * Rpsi : b -> e   ********************************)

Definition Rbe_phi_theta_psi :=  (Rbe_phi RM* Rbe_theta) RM* Rbe_psi.

Definition Rbe_phi_theta_psi_re:= mkMat_3_3
  ( cos theta * cos psi )( -cos theta * sin psi )( sin theta )
  ( sin phi * sin theta * cos psi + cos phi * sin psi)
  (   -sin phi * sin theta * sin psi + cos phi * cos psi)
  (   - sin phi * cos theta )
  ( - cos phi * sin theta * cos psi + sin phi * sin psi)
  (   cos phi * sin theta * sin psi + sin phi * cos psi)
  (   cos phi * cos theta ).

Lemma Rbe_phi_theta_psi_eq : Rbe_phi_theta_psi === Rbe_phi_theta_psi_re .
Proof. RMat_mul_simpl. unfold mkMat_3_3'. 
  f_equal2. ring. f_equal. ring. f_equal. ring.
  f_equal. ring. f_equal2. ring. f_equal. ring. 
Qed.

(**************************   Rphi * Rpsi * Rtheta : b -> e   ********************************)

Definition Rbe_phi_psi_theta := (Rbe_phi RM* Rbe_psi) RM* Rbe_theta.

Definition Rbe_phi_psi_theta_re:= mkMat_3_3
  ( cos psi * cos theta)( - sin psi )( cos psi * sin theta )
  ( cos phi * sin psi * cos theta + sin phi * sin theta)
  (   cos phi * cos psi)
  (   cos phi * sin psi * sin theta - sin phi * cos theta )
  ( sin phi * sin psi * cos theta - cos phi * sin theta )
  (   sin phi * cos psi )
  (   sin phi * sin psi * sin theta + cos phi * cos theta).

Lemma Rbe_phi_psi_theta_eq :Rbe_phi_psi_theta === Rbe_phi_psi_theta_re .
Proof. RMat_mul_simpl. unfold mkMat_3_3'. 
  f_equal2. ring. f_equal2. ring. f_equal. ring.
  f_equal2. ring. f_equal2. ring. 
Qed.

(**************************   Rpsi * Rtheta * Rphi : b -> e   ********************************)

Definition Rbe_psi_theta_phi := (Rbe_psi RM* Rbe_theta) RM* Rbe_phi.

Definition Rbe_psi_theta_phi_re:= mkMat_3_3
  ( cos psi * cos theta )
  (   -sin psi * cos phi + cos psi * sin theta * sin phi)
  (   sin psi * sin phi + cos psi * sin theta * cos phi )
  ( sin psi * cos theta )
  (   cos psi * cos phi + sin psi * sin theta * sin phi)
  (   -cos psi * sin phi + sin psi *sin theta * cos phi)
  ( - sin theta )( cos theta *sin phi )( cos theta * cos phi ).

Lemma Rbe_psi_theta_phi_eq : Rbe_psi_theta_phi === Rbe_psi_theta_phi_re .
Proof. RMat_mul_simpl. unfold mkMat_3_3'. 
  f_equal3. f_equal. ring. f_equal2. ring. f_equal2. ring.
  f_equal. ring. 
Qed.


(**************************   Rpsi * Rphi * Rtheta : b -> e   ********************************)

Definition Rbe_psi_phi_theta := (Rbe_psi RM* Rbe_phi) RM* Rbe_theta.

Definition Rbe_psi_phi_theta_re:= mkMat_3_3
  ( cos psi * cos theta - sin psi * sin phi * sin theta)
  (   - sin psi * cos phi)
  (   cos psi * sin theta + sin psi * sin phi * cos theta )
  ( sin psi * cos theta + cos psi * sin phi * sin theta )
  (   cos psi * cos phi )
  (   sin psi * sin theta - cos psi * sin phi * cos theta )
  ( - cos phi * sin theta )( sin phi )( cos phi * cos theta ).

Lemma Rbe_psi_phi_theta_eq : Rbe_psi_phi_theta === Rbe_psi_phi_theta_re .
Proof. RMat_mul_simpl. unfold mkMat_3_3'. 
  f_equal2. ring. f_equal2. ring. f_equal2. ring. f_equal. ring.
  f_equal2. ring. f_equal. f_equal. ring.  
Qed.


(**************************    Rtheta : e -> b   ********************************)

(* 在俯仰方向旋转theta角后机体坐标系到地球固连坐标系的转换矩阵(类型:矩阵 RMat 3 3) *)
Definition Reb_theta :=mkMat_3_3
  ( cos theta ) 0 (  -sin theta )
        0       1       0     
  ( sin theta ) 0 (  cos theta  ).


(**************************    Rphi : e -> b   ********************************)

(* 在滚转方向旋转phi角后机体坐标系到地球固连坐标系的转换矩阵(类型:矩阵 RMat 3 3) *)
Definition Reb_phi :=mkMat_3_3
   1     0       0   
   0 ( cos phi )( sin phi )
   0 ( -sin phi )(  cos phi).


(**************************    Rpsi : e -> b   ********************************)

(* 在偏航方向旋转psi角后机体坐标系到地球固连坐标系的转换矩阵(类型:矩阵 RMat 3 3) *)
Definition Reb_psi:= mkMat_3_3
  ( cos psi )( sin psi ) 0 
  ( -sin psi )( cos psi ) 0 
      0        0          1 .


(**************************   Rtheta * Rphi : e -> b   ********************************)

Definition Reb_theta_phi := Reb_theta RM* Reb_phi.

Definition Reb_theta_phi_re := mkMat_3_3
  (cos theta )( sin theta * sin phi)( - sin theta * cos phi)
   0 (cos phi)( sin phi)
  ( sin theta )( -cos theta * sin phi)( cos theta * cos phi).

Lemma Reb_theta_phi_eq :Reb_theta_phi === Reb_theta_phi_re.
Proof. RMat_mul_simpl. unfold  mkMat_3_3'.
  f_equal3. ring. f_equal2. ring. Qed.


(**************************   Rtheta * Rpsi : e -> b   ********************************)

Definition Reb_theta_psi := Reb_theta RM* Reb_psi.

Definition Reb_theta_psi_re := mkMat_3_3
  (cos theta *cos psi )( cos theta * sin psi)( -sin theta )
  ( -sin psi)(cos psi) 0 
  ( sin theta * cos psi )( sin theta * sin psi)(cos theta ).

Lemma Reb_theta_psi_eq : Reb_theta_psi === Reb_theta_psi_re.
Proof. RMat_mul_simpl. unfold  mkMat_3_3'.
  f_equal3. Qed.

(**************************   Rphi * Rtheta : e -> b   ********************************)

Definition Reb_phi_theta := Reb_phi RM* Reb_theta.

Definition Reb_phi_theta_re := mkMat_3_3
 (cos theta )0 ( -sin theta )
 ( sin phi * sin theta)( cos phi )( sin phi * cos theta)
 ( cos phi * sin theta )( -sin phi )( cos phi * cos theta ).

Lemma Reb_phi_theta_eq : Reb_phi_theta === Reb_phi_theta_re.
Proof. RMat_mul_simpl. unfold  mkMat_3_3'.
  f_equal3. Qed.

(**************************   Rphi * Rpsi : e -> b   ********************************)

Definition Reb_phi_psi := Reb_phi RM* Reb_psi.

Definition Reb_phi_psi_re := mkMat_3_3
   ( cos psi )(  sin psi ) 0 
   ( -cos phi * sin psi )( cos phi * cos psi )(  sin phi )
   ( sin phi * sin psi )( -sin phi * cos psi )(   cos phi ).

Lemma Reb_phi_psi_eq : Reb_phi_psi === Reb_phi_psi_re.
Proof. RMat_mul_simpl. unfold  mkMat_3_3'.
  f_equal3. ring. f_equal. ring.
Qed.

(**************************   Rpsi * Rtheta : e -> b   ********************************)

Definition Reb_psi_theta := Reb_psi RM* Reb_theta.

Definition Reb_psi_theta_re := mkMat_3_3
   ( cos psi * cos theta )( sin psi )( -cos psi * sin theta )
   ( -sin psi * cos theta )( cos psi )( sin psi * sin theta )
   (sin theta ) 0 ( cos theta ).

Lemma Reb_psi_theta_eq : Reb_psi_theta === Reb_psi_theta_re.
Proof. RMat_mul_simpl. unfold  mkMat_3_3'.
  f_equal3. f_equal. ring. f_equal2. ring. Qed.

(**************************   Rpsi * Rphi : e -> b   ********************************)

Definition Reb_psi_phi := Reb_psi RM* Reb_phi.

Definition Reb_psi_phi_re :=  mkMat_3_3
   (cos psi )(  sin psi * cos phi )( sin psi * sin phi )
   ( -sin psi )(  cos psi * cos phi )( cos psi * sin phi )
    0 ( -sin phi )( cos phi ).

Lemma Reb_psi_phi_eq : Reb_psi_phi === Reb_psi_phi_re.
Proof. RMat_mul_simpl. unfold  mkMat_3_3'.
  f_equal3. Qed.


(**************************   Rtheta * Rphi * Rpsi : e -> b   ********************************)

Definition Reb_theta_phi_psi :=(Reb_theta RM* Reb_phi) RM* Reb_psi.

Definition Reb_theta_phi_psi_re:=  mkMat_3_3
  ( cos theta * cos psi - sin theta * sin phi * sin psi) 
  (   cos theta * sin psi + sin theta * sin phi *cos psi) 
  (   -sin theta * cos phi)
  ( -cos phi * sin psi )( cos phi * cos psi )( sin phi)
  ( sin theta * cos psi + cos theta * sin phi *sin psi )
  (   sin theta * sin psi - cos theta * sin phi * cos psi)
  (   cos theta * cos phi).

Lemma Reb_theta_phi_psi_eq : Reb_theta_phi_psi === Reb_theta_phi_psi_re .
Proof. RMat_mul_simpl. unfold  mkMat_3_3'. 
  f_equal2. ring. f_equal. ring. f_equal. ring. f_equal. ring.
  f_equal2. ring. f_equal. ring. 
Qed.

(**************************   Rtheta * Rpsi * Rphi : e -> b   ********************************)

Definition Reb_theta_psi_phi := (Reb_theta RM* Reb_psi) RM* Reb_phi.

Definition Reb_theta_psi_phi_re:=  mkMat_3_3
  ( cos theta * cos psi )
  (   cos theta *sin psi * cos phi + sin theta *sin phi)
  (   cos theta * sin psi * sin phi - sin theta * cos phi)
  ( -sin psi)( cos psi * cos phi )(  cos psi *sin phi)
  (  sin theta * cos psi)
  (   sin theta *sin psi * cos phi - cos theta * sin phi)
  (   sin theta*sin psi * sin phi + cos theta * cos phi ).

Lemma Reb_theta_psi_phi_eq : Reb_theta_psi_phi === Reb_theta_psi_phi_re .
Proof. RMat_mul_simpl. unfold  mkMat_3_3'. 
  f_equal3. ring. f_equal. ring. f_equal. ring.
  f_equal. ring. f_equal2. ring.
Qed.

(**************************   Rphi * Rtheta * Rpsi : e -> b   ********************************)

Definition Reb_phi_theta_psi := (Reb_phi RM* Reb_theta) RM* Reb_psi.

Definition Reb_phi_theta_psi_re:=  mkMat_3_3
  ( cos theta * cos psi )( cos theta * sin psi )( -sin theta )
  ( sin phi * sin theta * cos psi - cos phi * sin psi)
  (   sin phi * sin theta * sin psi + cos phi * cos psi)
  (   sin phi * cos theta )
  ( cos phi * sin theta * cos psi + sin phi * sin psi)
  (   cos phi * sin theta * sin psi - sin phi * cos psi)
  (   cos phi * cos theta ).

Lemma Reb_phi_theta_psi_eq : Reb_phi_theta_psi === Reb_phi_theta_psi_re .
Proof. RMat_mul_simpl. unfold  mkMat_3_3'. 
  f_equal2. ring. f_equal. ring. f_equal. ring.
  f_equal2. ring. f_equal. ring.
Qed.

(**************************   Rphi * Rpsi * Rtheta : e -> b   ********************************)

Definition Reb_phi_psi_theta := (Reb_phi RM* Reb_psi) RM* Reb_theta.

Definition Reb_phi_psi_theta_re:=  mkMat_3_3
  ( cos psi * cos theta)( sin psi )( - cos psi * sin theta )
  ( - cos phi * sin psi * cos theta + sin phi * sin theta)
  (   cos phi * cos psi)
  (  cos phi * sin psi * sin theta + sin phi * cos theta )
  ( sin phi * sin psi * cos theta + cos phi * sin theta )
  (   - sin phi * cos psi )
  (  - sin phi * sin psi * sin theta + cos phi * cos theta ).


Lemma Reb_phi_psi_theta_eq : Reb_phi_psi_theta === Reb_phi_psi_theta_re .
Proof. RMat_mul_simpl. unfold  mkMat_3_3'. 
  f_equal2. ring. f_equal2. ring. f_equal3. ring. ring.
  f_equal3. ring. f_equal2. ring. 
Qed.

(**************************   Rpsi * Rtheta * Rphi : e -> b   ********************************)

Definition Reb_psi_theta_phi := (Reb_psi RM* Reb_theta) RM* Reb_phi.

Definition Reb_psi_theta_phi_re:=  mkMat_3_3
  ( cos psi * cos theta )
  (   sin psi * cos phi + cos psi * sin theta * sin phi)
  (   sin psi * sin phi - cos psi * sin theta * cos phi )
  ( - sin psi * cos theta) 
  (   cos psi * cos phi - sin psi * sin theta * sin phi)
  (  cos psi * sin phi + sin psi *sin theta * cos phi )
  ( sin theta )( - cos theta *sin phi )( cos theta * cos phi ).

Lemma Reb_psi_theta_phi_eq : Reb_psi_theta_phi === Reb_psi_theta_phi_re .
Proof. RMat_mul_simpl. unfold  mkMat_3_3'.  
  f_equal3. ring. f_equal. ring. f_equal. ring. f_equal2. ring.
  f_equal2. ring. f_equal. ring. 
Qed.

(**************************   Rpsi * Rphi * Rtheta : e -> b   ********************************)

Definition Reb_psi_phi_theta := (Reb_psi RM* Reb_phi) RM* Reb_theta.

Definition Reb_psi_phi_theta_re:=  mkMat_3_3
  ( cos psi * cos theta + sin psi * sin phi * sin theta)
  (   sin psi * cos phi)
  (   - cos psi * sin theta + sin psi * sin phi * cos theta )
  ( - sin psi * cos theta + cos psi * sin phi * sin theta )
  (   cos psi * cos phi )
  (   sin psi * sin theta + cos psi * sin phi * cos theta )
  ( cos phi * sin theta ) (- sin phi )( cos phi * cos theta ).


Lemma Reb_psi_phi_theta_eq :Reb_psi_phi_theta === Reb_psi_phi_theta_re .
Proof. RMat_mul_simpl. unfold  mkMat_3_3'. 
  f_equal3. f_equal2. ring. f_equal2. ring. f_equal. ring. f_equal2. ring.
Qed.

End CoordinateTransformationMatrix.

Section CTMatrix_lemma.

(* 相同角度，机体坐标系到地球固连坐标系的旋转矩阵 与 地球固连坐标系到机体坐标系的旋转矩阵
   的乘积为单位矩阵，即 Rbe*Reb = E*)
Lemma Rbe_Reb_I : forall theta, (Rbe_theta theta) RM* (Reb_theta theta ) === RMI 3.
Proof. 
  intros. RMat_mul_simpl. RMI_simpl. Rtrigo_simpl. f_equal3. 
  f_equal. ring. f_equal. ring.
Qed.

End CTMatrix_lemma.
  